#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <string.h>
#include <unistd.h>  // for sleep()
#include "fifo.h"
#include "../msg.h"


static int is_power_of_two(unsigned int num)
{
    if(num == 0)
        return 0;
    return !(num & (num - 1));
}

static  int  up_power_of_two(unsigned int num) {
    if (num == 0) return 1;  // 最小返回1
    return 1 << (32 - __builtin_clz(num - 1));
}
struct kfifo * kfifo_alloc(unsigned int width,unsigned int height,unsigned int num)
{

    num = up_power_of_two(num);
    struct kfifo *fifo = malloc(sizeof(struct kfifo));
    if (!fifo) return NULL;
    memset(fifo,0,sizeof(struct kfifo));
    fifo->buffer = malloc(width * height * FIFO_FORMAT * num);
    if (!fifo->buffer) {
        free(fifo);
        return NULL;
    }
    memset(fifo->buffer ,0,width * height * FIFO_FORMAT * num);
    fifo->msg_buffer = malloc(sizeof(struct carama_msg) * num);

    if(!fifo->msg_buffer) {
        free(fifo);
        free(fifo->buffer);
        return NULL;
    }
    memset(fifo->msg_buffer,0,sizeof(unsigned int) * num);
    pthread_mutex_init(&fifo->lock,NULL);
    pthread_cond_init(&fifo->not_empty,NULL);
    fifo->fifo_num = num;
    fifo->fifo_size = width * height * FIFO_FORMAT * num;
    fifo->write_index = 0;
    fifo->sigle_size = width * height * FIFO_FORMAT;
    return fifo;
}

int kfifo_write(struct kfifo *fifo, void *buffer, struct carama_msg *send_msg) {
    pthread_mutex_lock(&fifo->lock);  // 获取写锁
    send_msg->real_size = MIN(send_msg->real_size, fifo->sigle_size);
    char (*buffer_array)[fifo->sigle_size] = fifo->buffer;
    char *write_pos = buffer_array[fifo->write_index & (fifo->fifo_num - 1)];
    memcpy(write_pos, buffer, send_msg->real_size);
    struct carama_msg * msg_buffer = (struct carama_msg *)fifo->msg_buffer;
    msg_buffer[fifo->write_index & (fifo->fifo_num - 1)].width = send_msg->width;
    msg_buffer[fifo->write_index & (fifo->fifo_num - 1)].height = send_msg->height;
    msg_buffer[fifo->write_index & (fifo->fifo_num - 1)].real_size = send_msg->real_size;  
    fifo->write_index++;

    pthread_cond_broadcast(&fifo->not_empty);
    pthread_mutex_unlock(&fifo->lock);  // 获取写锁
    return send_msg->real_size ;
}

int kfifo_read(struct kfifo *fifo, void *buffer, unsigned int size, 
                struct carama_msg *recv_msg,unsigned int *read_index) {
    pthread_mutex_lock(&fifo->lock);
    // 如果没有数据或者有写线程正在进行写操作，阻塞读线程
    while (fifo->write_index == *read_index) {
        pthread_cond_wait(&fifo->not_empty, &fifo->lock);
    }
    // 等待 FIFO 中有数据
    // 读取数据
    size = MIN(size, fifo->sigle_size);
    unsigned int real_size = ((struct carama_msg*)fifo->msg_buffer)[*read_index & (fifo->fifo_num - 1)].real_size;
    size = MIN(size,real_size);
    recv_msg->real_size = size;
    recv_msg->height = ((struct carama_msg*)fifo->msg_buffer)[*read_index & (fifo->fifo_num - 1)].height;
    recv_msg->width = ((struct carama_msg*)fifo->msg_buffer)[*read_index & (fifo->fifo_num - 1)].width;

    char (*buffer_array)[fifo->sigle_size] = fifo->buffer;
    char *read_pos = buffer_array[*read_index & (fifo->fifo_num - 1)];
    *read_index += 1;
    memcpy(buffer, read_pos, size);
    pthread_mutex_unlock(&fifo->lock);
    return size;
}


void kfifo_free(struct kfifo *fifo) {
    if (fifo) {
        pthread_mutex_destroy(&fifo->lock);
        pthread_cond_destroy(&fifo->not_empty);
        free(fifo->buffer);
        free(fifo->msg_buffer);
        free(fifo);
    }
}

